#include "rtx.h"
#include "initialization.h"

#include "debug.h"
#include "memstack.h"
#include "proctables.h"
#include "pcbqueue.h"
#include "globals.h"
#include "pcb.h"

VOID empty() {}

UINT32 init_memory (VOID)
{
	UINT32 i;
	UINT32 mem = (UINT32)&(__end);	// Thank you mapping file
	mem_head = NULL; // Initialize our stack pointer for our memory

	avail_mem = NUM_MEMORY_BLOCKS;

	for(i = 0; i < NUM_MEMORY_BLOCKS ; i++)
	{
		push((MEMORY*)mem);
		mem += sizeof (struct memory_block);
	}

	return (UINT32)mem;
}

VOID init_interrupts (VOID)
{
	UINT32 interrupt_mask;

	asm("move.l %d0, -(%a7)");

	// Initialize our trap handler
	asm("move.l #swi_trap,%d0");
	asm("move.l %d0,0x10000080");

	asm("move.l (%a7)+, %d0");

	// Now for the timer initialization


	// The next assembly codes enable the timer interrupts
    asm("move.l #timer_handler,%d0");
    asm("move.l %d0,0x10000078");

	// Mask the timer to level 6, priority 3
	TIMER0_ICR = 0x9B;

	// Set the TRR to an approximate 10ms count
	TIMER0_TRR = 1758;

    // Initialize the timer
	TIMER0_TMR = 0xFF1B;

	// Mask the timer interrupt
	interrupt_mask = SIM_IMR;
	interrupt_mask &= 0x0003fdff;
	SIM_IMR = interrupt_mask;

	// Now for the UART initialiazation

	// Set up the UART interrupts
    asm( "move.l #uart_handler,%d0" );
    asm( "move.l %d0,0x10000100" );

	// Reset the UART, the receiver, transmit and error cond
    SERIAL1_UCR = 0x10;
    SERIAL1_UCR = 0x20;
    SERIAL1_UCR = 0x30;
    SERIAL1_UCR = 0x40;

    // Install the interupt
    SERIAL1_ICR = 0x17;
    SERIAL1_IVR = 64;

    // enable interrupts on rising edge
    //SERIAL1_IMR = 0x02;
    SERIAL1_IMR = 3;


    // Setup for transmit and receive
    SERIAL1_UCR = 0x05;

    // Mask the interrupts for the UART
    interrupt_mask = SIM_IMR;
    interrupt_mask &= 0x0003dfff;
    SIM_IMR = interrupt_mask;

    //Renable interrupts
    //asm( "move.w #0x2000,%sr");
}

VOID init_queues (VOID) {

	UINT32 i = 0;
	while(i < NUM_PRIORITIES) {
		blocked_on_resource_queue.queues[i].head = NULL;
		blocked_on_resource_queue.queues[i].tail = NULL;

		ready_queue.queues[i].head = NULL;
		ready_queue.queues[i].tail = NULL;

		i++;
	}
}

VOID init_processes (UINT32 stack_start) {
	screen_ready = FALSE;
	UINT32 stack = stack_start;

	UINT32 i = 0;
	while(i < NUM_PROCESSES) {

		debug_out_int(i);
		debug_out_string("\n\r");

		empty();

		pcbs[i].message_queue_head = NULL;
		pcbs[i].message_queue_tail = NULL;
		pcbs[i].stack_pointer = NULL;
		pcbs[i].program_counter = NULL;
		pcbs[i].status_register = NULL;
		pcbs[i].next = NULL;

		pcbs[i].process_ID = RTX_PROCS[i].pid;
		pcbs[i].priority = NULL;
		pcbs[i].priority = (RTX_PROCS[i].priority);

		pcbs[i].stack_size = RTX_PROCS[i].stacksize;

		stack += pcbs[i].stack_size;
		pcbs[i].stack_pointer = (UINT32*)stack;

		pcbs[i].program_counter = (UINT32*)RTX_PROCS[i].rtx_proc_entry;

		if(pcbs[i].process_ID != TIMER_I_PROCESS && pcbs[i].process_ID != UART_I_PROCESS) {
			pcbs[i].status_register = 0x45672000;
			pcbs[i].process_state = READY;

			if(pcbs[i].process_ID != NULL_PROCESS) {
				rpq_enqueue(&(pcbs[i]));
			}
		} else {
			pcbs[i].status_register = 0x45672700;
			pcbs[i].process_state = I_PROCESS;
		}

		init_process(pcbs[i].process_ID);

		i++;
	}
}

VOID init_process (UINT32 processID) {
	//write to stack so context_switch doesn't screw up.

	UINT32* temp = 0;

	asm( "move.l %a0, -(%a7)" );

	temp = get_pcb(processID)->stack_pointer;

	asm( "move.l -4(%a6), %a0" );

	asm( "move.l #0, -(%a0)" );
	asm( "move.l -4(%a6), -(%a0)" );

	asm( "move.l #0, -(%a0)" );
	asm( "move.l #0, -(%a0)" );
	asm( "move.l #0, -(%a0)" );

	asm( "move.l #0, -(%a0)" );
	asm( "move.l #0, -(%a0)" );
	asm( "move.l #0, -(%a0)" );
	asm( "move.l #0, -(%a0)" );
	asm( "move.l #0, -(%a0)" );
	asm( "move.l #0, -(%a0)" );
	asm( "move.l #0, -(%a0)" );
	asm( "move.l #0, -(%a0)" );
	asm( "move.l #0, -(%a0)" );
	asm( "move.l #0, -(%a0)" );
	asm( "move.l #0, -(%a0)" );
	asm( "move.l #0, -(%a0)" );
	asm( "move.l #0, -(%a0)" );
	asm( "move.l #0, -(%a0)" );

	asm( "move.l %a0, -4(%a6)" );

	get_pcb(processID)->stack_pointer = temp;

	asm( "move.l (%a7)+, %a0" );
}
